
#include <math.h>
#include "3d_math.h"

#include <stdio.h>
#include "3d_unit.h"

//重置
void D3Unit::Reset(float x, float y, float z,
    float turnUpDown, float turnLeftRight, float rollLeftRight)
{
    xyz[0] = xyz[1] = xyz[2] = 0;
    quat[0] = 1; quat[1] = quat[2] = quat[3] = 0;
    speed[0] = speed[1] = speed[2] = 0;
    accel[0] = accel[1] = accel[2] = 0;

    SetLocation(x, y, z);
    SetDirection(turnUpDown, turnLeftRight, rollLeftRight);
}

//相对位置转换(把空间坐标转换为相对unit的坐标,即以unit为坐标原点的坐标)
void D3Unit::Transfer(float xyz[3])
{
    //先平移(注意是反向平移)
    xyz[0] -= this->xyz[0];
    xyz[1] -= this->xyz[1];
    xyz[2] -= this->xyz[2];
    //再旋转(注意是反向旋转)
    quat_roll(quat, NULL, 0, xyz, true);
}

//空间坐标移动
void D3Unit::Move(float x, float y, float z)
{
    xyz[0] += x;
    xyz[1] += y;
    xyz[2] += z;
}

//按自己朝向移动
void D3Unit::MoveSelf(float frontBack, float leftRight, float upDown)
{
    float v[] = {frontBack, leftRight, upDown};
    //先把平移向量旋转为相机坐标系
    quat_roll(quat, NULL, 0, v, false);
    //再加到相机坐标中
    Move(v[0], v[1], v[2]);
}

//朝指定目标移动
void D3Unit::MoveTo(float xyz[3], float distance)
{
    //平移向量
    float v[] = {
        xyz[0] - this->xyz[0],
        xyz[1] - this->xyz[1],
        xyz[2] - this->xyz[2]};
    //计算向量模
    float norm = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
    if (isnan(norm))
        return;
    //向量单位化后再乘以 distance
    v[0] = v[0] * distance / norm;
    v[1] = v[1] * distance / norm;
    v[2] = v[2] * distance / norm;
    //再加到相机坐标中
    Move(v[0], v[1], v[2]);
}

//绕空间坐标轴旋转,单位:角度(以自身为原点,绕平行于空间坐标系的三轴旋转)
void D3Unit::Roll(float x, float y, float z)
{
    float v[] = {x, y, z};
    //先旋转为相机坐标系
    quat_roll(quat, NULL, 0, v, true);
    //再进行自身旋转
    RollSelf(v[1], v[2], v[0]);
}

//绕自身坐标轴旋转,单位:角度
void D3Unit::RollSelf(float turnUpDown, float turnLeftRight, float rollLeftRight)
{
    float v[] = {rollLeftRight, -turnUpDown, turnLeftRight};
    //四元数增量
    quat_diff2(quat, v);
}

//绕指定向量旋转,单位:角度
// void D3Unit::RollAround(float vector[3], float angle)
// {
//     ;
// }

//直接设置位置
void D3Unit::SetLocation(float x, float y, float z)
{
    xyz[0] = x;
    xyz[1] = y;
    xyz[2] = z;
}

//直接设置朝向
void D3Unit::SetDirection(float turnUpDown, float turnLeftRight, float rollLeftRight)
{
    float roll_xyz[] = {rollLeftRight, -turnUpDown, turnLeftRight};
    pry_to_quat2(roll_xyz, quat);
}
